
#include <float.h>
#include <BSolverItem>
#include "member_BBody.h"
#include "member_BNode.h"
#include "member_BShape.h"
#include "member_BRestraint.h"

using namespace BWE;

BBody* BSolverItem::bodyA()
{
	if (contact)
		return contact->bodyA;
	if (restraint)
		return restraint_member(restraint)->bodyA;
	return 0;
}
BBody* BSolverItem::bodyB()
{
	if (contact)
		return contact->bodyB;
	if (restraint)
		return restraint_member(restraint)->bodyB;
	return 0;
}

BVector BSolverItem::angularAccA(const BVector& vector)
{
	if (BBody* body = bodyA())
	{
		const BMatrix& matrix = node_member(body)->matrix;
		BVector acc = matrix.inverse().mult3(vector) * invInertiaA;
		return matrix.mult3(acc);
	}
	return BVector();
}
BVector BSolverItem::angularAccB(const BVector& vector)
{
	if (BBody* body = bodyB())
	{
		const BMatrix& matrix = node_member(body)->matrix;
		BVector acc = matrix.inverse().mult3(vector) * invInertiaB;
		return matrix.mult3(acc);
	}
	return BVector();
}
